
(define-library (turtle physics)
  (import
    (scheme base)
    (scheme inexact)
    (turtle turtle3)
    (turtle vector)
    (yasos))
  (export apply-force apply-momentum
          planet? get-mass get-momentum get-force
          set-mass! set-momentum! set-force! apply-physics!
          calculate-gravity make-planet)

  (begin
    (define (apply-force f-vector p-vector delta-t)
      ;; F * dt = dp
      (vector-sum p-vector (scale-vector f-vector delta-t)))
    
    (define (apply-momentum p-vec start-pos mass delta-t)
      ;; ds = (p / m) * dt
      (vector-sum start-pos (scale-vector p-vec (/ delta-t mass))))
    
    (define-predicate planet?)
    (define-operation (get-mass obj))
    (define-operation (get-momentum obj))
    (define-operation (get-force obj))
    (define-operation (set-mass! obj new-mass))
    (define-operation (set-momentum! obj new-momentum))
    (define-operation (set-force! obj new-force))
    (define-operation (apply-physics! obj delta-t))

    (define (calculate-gravity body-1 body-2 g-constant)
      ;; g-constant is the gravitational constant
      ;; this returns the force acting upon body-1
      (define d (vector-distance (get-pos body-1)
                                 (get-pos body-2)))
      (define f-magnitude (/ (* g-constant
                                (get-mass body-1)
                                (get-mass body-2))
                             (square d)))
      (define difference-vector (vector-sum (get-pos body-2)
                                            (scale-vector (get-pos body-1) -1)))
      (define unit-difference (unit-vector difference-vector))
      (scale-vector unit-difference f-magnitude))

    (define (make-planet)
      (define mass 1.0)
      (define momentum #(0.0 0.0 0.0))
      (define force #(0.0 0.0 0.0))
      (object-with-ancestors ((p (make-turtle)))
        ((planet? self) #t)
        ((get-mass self) mass)
        ((get-momentum self) momentum)
        ((get-force self) force)
        ((set-mass! self new-mass)
         (set! mass new-mass))
        ((set-momentum! self new-momentum)
         (set! momentum new-momentum))
        ((set-force! self new-force)
         (set! force new-force))
        ((apply-physics! self delta-t)
         (set! momentum (apply-force (get-force self) (get-momentum self) delta-t))
         (set-pos! self (apply-momentum (get-momentum self)
                                        (get-pos self)
                                        (get-mass self)
                                        delta-t)))))))

